#include "asm/iic_soft.h"
#include "as5600.h"

#define abs(x) ((x) > 0 ? (x) : -(x))
#define _2PI 6.28318530718

#define iic_hdl 0

#define i2c_Start() soft_iic_start(iic_hdl)
#define i2c_SendByte(byte) soft_iic_tx_byte(iic_hdl, byte)
#define i2c_Stop() soft_iic_stop(iic_hdl)
#define i2c_ReadByte(ack) soft_iic_rx_byte(iic_hdl, ack)
#define i2c_Read_buffer_Byte(buf, len) soft_iic_read_buf(iic_hdl, buf, len)

static float angle_data_prev;      // 上次位置
static float full_rotation_offset; // 转过的整圈数

void bsp_as5600Init(void)
{
    /* init i2c interface */

    /* init var */
    full_rotation_offset = 0;
    angle_data_prev = bsp_as5600GetRawAngle();
}
u8 as5600_write_data(u8 addr, u8 data)
{
    i2c_Start();
    if (!i2c_SendByte(AS5600_WRITE_ADDR))
    {
        printf("send id erro!");
        return 0;
    }
    if (!i2c_SendByte(addr))
    {
        printf("send addr erro!");
        return 0;
    }
    i2c_SendByte(data);
    i2c_Stop();
    return 1;
}

u8 as5600_read_data(u8 addr, u8 *data, u8 len)
{

    i2c_Start();
    // 发命令
    if (!i2c_SendByte(AS5600_WRITE_ADDR))
    {
        printf("send id erro!");
        return 0;
    }

    if (!i2c_SendByte(addr))
    {
        printf("send id erro!");
        return 0;
    }

    i2c_Start();
    // 发命令
    if (!i2c_SendByte(AS5600_READ_ADDR))
    {
        printf("send id erro!");
        return 0;
    }

    for (u8 i = 0; i < len; i++)
    {
        if (i == len - 1)
        {
            data[i] = i2c_ReadByte(0);
        }
        else
        {
            data[i] = i2c_ReadByte(1);
        }
    }
    i2c_Stop();
    return 1;
}

uint16_t bsp_as5600GetRawAngle(void)
{
    uint16_t raw_angle;
    uint8_t buffer[2] = {0};
    as5600_read_data(AS5600_RAW_ANGLE_REGISTER, buffer, 2);
    raw_angle = buffer[0] << 8 | buffer[1];
    printf("get raw angle %d %X %X", raw_angle, buffer[0], buffer[1]);
    raw_angle = ((uint16_t)buffer[0] << 8) | (uint16_t)buffer[1];
    return raw_angle;
}

u16 bsp_as5600GetAngle(void)
{
    uint16_t raw_angle;
    u16 angle = 0;
    uint8_t buffer[2] = {0};
    as5600_read_data(AS5600_ANGLE_REGISTER, buffer, 2);
    angle = buffer[0] << 8 | buffer[1];
    angle = angle / 11.375;
    printf("                             get angle %d %X %X", angle, buffer[0], buffer[1]);

    return angle;
}
